#include <stdio.h>
#include "dijkstra.h"


typedef struct 
{
   double coordenada_x;
   double coordenada_y;
} produto_cartesiano;

void converte_matocup_matcus(int matriz_ocupacao[TAMANHO_OCUPACAO][TAMANHO_OCUPACAO], int matriz_custos[TAMANHO_CUSTOS][TAMANHO_CUSTOS])
{
  int i, j;
  
  for (i = 0; i < TAMANHO_CUSTOS; i++)
    for (j = 0; j < TAMANHO_CUSTOS; j++)  
	if (matriz_ocupacao[i/TAMANHO_OCUPACAO][i%TAMANHO_OCUPACAO] == -1 && matriz_ocupacao[j/TAMANHO_OCUPACAO][j%TAMANHO_OCUPACAO] == -1 && ((i == (j + 1) && (i % TAMANHO_OCUPACAO != 0)) || (i == (j - 1) && (i % TAMANHO_OCUPACAO != (TAMANHO_OCUPACAO - 1))) || (i == (j + TAMANHO_OCUPACAO)) || (i == (j - TAMANHO_OCUPACAO))))
	{
	   //printf("\nCaminho enconrado de %d para %d ", i, j);
	   //getchar();
	   matriz_custos[i][j] = 1;
	}
	else
	  matriz_custos[i][j] = -1;
}


int main()
{
  
  int i,j, numero_vertices;
  int matriz_ocupacao[TAMANHO_OCUPACAO][TAMANHO_OCUPACAO];
  int matriz_custos[TAMANHO_CUSTOS][TAMANHO_CUSTOS];
  int *matriz_custos_linearizada;
  int *caminho;
  double ponto_central;
  produto_cartesiano quadrados[25];
  
  
  matriz_custos_linearizada = (int *)malloc (sizeof(int)*(TAMANHO_CUSTOS*TAMANHO_CUSTOS));
  caminho = (int *)malloc (sizeof(int)*(TAMANHO_CUSTOS));
  
  for (i = 0; i < TAMANHO_OCUPACAO; i++)
    for (j = 0; j < TAMANHO_OCUPACAO; j++)
       if ((i == 1 && j == 1) || (i == 1 && j == 2) || (i == 2 && j == 1) || (i == 2 && j == 2) || (i == 3 && j == 1) || (i == 3 && j == 2))
	  matriz_ocupacao[i][j] = 1;
       else
	  matriz_ocupacao[i][j] = -1;
  
  converte_matocup_matcus(matriz_ocupacao, matriz_custos);
 
  //printf("\n Matriz Inicial de custos");
  for (i = 0; i < TAMANHO_CUSTOS; i++)
     for (j = 0; j <  TAMANHO_CUSTOS; j++)
	 matriz_custos_linearizada[i * TAMANHO_CUSTOS + j] = matriz_custos[i][j];

  caminho = dijkstra(TAMANHO_CUSTOS,13,16,matriz_custos_linearizada,&numero_vertices);
  
  //printf("\nCaminho retornado por Dijkstra: \n");
  
  //for (i = (numero_vertices-1); i >= 0 ; i--)
     //printf("%d ", caminho[i]);
  
  //printf("\nNúmero de vértices: %d", numero_vertices);
  
  //ponto_central = (TAMANHO_OCUPACAO/2) - 1;
  ponto_central = (TAMANHO_OCUPACAO/2.0) - 1;
  printf("\nPonto central: %f\n", ponto_central);
  
  for (i = 0; i < TAMANHO_CUSTOS; i++)
  {
      quadrados[i].coordenada_x = (((i % TAMANHO_OCUPACAO) - ponto_central) - 0.5);
      quadrados[i].coordenada_y = (((i / TAMANHO_OCUPACAO) - ponto_central) - 0.5);
      
      printf("Coordenadas do quadrado %d: (%f,%f)\n", i, quadrados[i].coordenada_x, quadrados[i].coordenada_y);
  }
  
    printf("Coordenadas dos quadrados do caminho: \n");
    for (i = (numero_vertices-1); i >= 0 ; i--)
      printf("(%f,%f) ", quadrados[caminho[i]-1].coordenada_x, quadrados[caminho[i]-1].coordenada_y);
    printf("\n");
  
  
  return 0;
}